/*
 * CameraSystem.cpp
 *
 *  Created on: Jan 22, 2011
 *      Author: user
 */

#include "CameraSystem.h"
#include "Constants.h"
#include "math.h"





//
CameraSystem::CameraSystem(): m_horizServo(4,9), m_vertServo(4,10), m_location(-10.5f,9.5f,8.0f){

	m_horizServo.SetAngle((m_horizServo.GetMaxAngle() + m_horizServo.GetMinAngle()) / 2);
	m_vertServo.SetAngle((m_vertServo.GetMaxAngle() + m_vertServo.GetMinAngle()) / 2);
}
//
CameraSystem::~CameraSystem(){

}
//
void CameraSystem::setLocation(Point3D& pt){
	m_location.moveTo(pt);
}
//
void CameraSystem::initCamera(){
	mptr_camera = &AxisCamera::GetInstance();
	mptr_camera->WriteResolution(AxisCamera::kResolution_320x240);
	mptr_camera->WriteCompression(20);
	mptr_camera->WriteRotation(AxisCamera::kRotation_180);
}

//
void CameraSystem::lookAt(Point3D pt){
	//Get vector from here to there (Rectangular)
	//SOLUTION: Vector angles always come out to (0,0)
	//SOLUTION: They were not being converted back into degrees from radians
	Vector v(m_location,pt);
	//Figure out what the Spherical form of the vector is
	//Get the theta and phi
	//Normalize to match the measures of the servos
	lookAngles(v.getTheta(),v.getPhi());
}

//
/*
 * Theta = horizontal angle with straight right being 0
 * phi =  vertical angle with straight up being 0
 */
void CameraSystem::lookAngles(float theta, float phi){
	//map theta/phi to servo angles
	theta = normalizeAngle(theta);
	float servoCenter = (m_horizServo.GetMaxAngle() + m_horizServo.GetMinAngle())/2;
	float servoTheta = servoCenter +90 -theta;
	if (servoTheta > m_horizServo.GetMaxAngle()) servoTheta = m_horizServo.GetMaxAngle();
	else if (servoTheta < m_horizServo.GetMinAngle()) servoTheta = m_horizServo.GetMinAngle();
	m_horizServo.SetAngle(servoTheta);

	phi = normalizeAngle(phi);
	/*float*/ servoCenter = (m_vertServo.GetMaxAngle() + m_vertServo.GetMinAngle())/2;
	float servoPhi = servoCenter +90 -phi;
	if (servoPhi > m_vertServo.GetMaxAngle()) servoPhi = m_vertServo.GetMaxAngle();
	else if (servoPhi < m_vertServo.GetMinAngle()) servoPhi = m_vertServo.GetMinAngle();
	m_vertServo.SetAngle(servoPhi);
}


void CameraSystem::panTilt(float dtheta, float dphi){
	if(fabs(dtheta) <= CAMERA_DEADBAND){
		dtheta = 0.0f;
	}
	if(fabs(dphi) <= CAMERA_DEADBAND){
		dphi = 0.0f;
	}
	m_horizServo.SetAngle(m_horizServo.GetAngle() + dtheta);
	m_vertServo.SetAngle(m_vertServo.GetAngle() + dphi);
}






